 #include "rostopic.h"

#include <QDebug>

ROSTopic::ROSTopic(int argc, char *argv[]) :
    _wheel_separation(334),
    _wheel_radius(75),
    _reduction(16)
{
    _argc = argc;
    _argv = argv;
    _lastTime = QTime::currentTime();
}

void ROSTopic::run()
{
    ros::init(_argc, _argv, "listener");
    ros::NodeHandle n;

    n.param("wheel_separation", _wheel_separation, _wheel_separation);
    n.param("wheel_radius",_wheel_radius,_wheel_radius);
    n.param("reduction",_reduction,_reduction);

    ros::Subscriber odom_sub = n.subscribe("odom", 10, &ROSTopic::odomCallback, this);
    ros::Subscriber battery_sub = n.subscribe("battery", 10, &ROSTopic::batteryCallback, this);
    ros::spin();
}

void ROSTopic::odomCallback(const nav_msgs::Odometry::ConstPtr &odom)
{
    setPositionX(odom->pose.pose.position.x);
    setPositionY(odom->pose.pose.position.y);
    setYaw(tf::getYaw(odom->pose.pose.orientation));

    setLinearX(odom->twist.twist.linear.x);
    setAngularZ(odom->twist.twist.angular.z);
    setAngularX(odom->twist.twist.angular.x);
    double vel_x = odom->twist.twist.linear.x * 1000;
    double vel_th = odom->twist.twist.angular.z;   //1000rad/s,实际角速度乘以 1000 发逿
    double pv[2] = {0.0,0.0};
    double  pw[2];
    pv[0] = (double)(vel_x - (double)(vel_th * _wheel_separation) / (double)2);
    pv[1] = (double)(vel_x + (double)(vel_th * _wheel_separation) / (double)2);
    for (int i = 0; i < 2; i++) {
      pw[i] = (double)(pv[i]/ _wheel_radius); //jiansubi 1:20
    }
    int rpm_l = (int)((pw[0]*(((double)30)*_reduction))/(double)PI);
    int rpm_r = (int)((pw[1]*(((double)30)*_reduction))/(double)PI);
    setLeftrmp(rpm_l);
    setRightrmp(rpm_r);

    setTimeStamp(odom->header.seq);
    QTime _currentTime = QTime::currentTime();
    setFrequency((int)(1/((double)_lastTime.msecsTo(_currentTime)/1000)));
    _lastTime = QTime::currentTime();
}

void ROSTopic::batteryCallback(const sensor_msgs::BatteryState::ConstPtr &battery)
{
    setVoltage(battery->voltage);
    setCurrent(battery->current);
    setCharge(battery->charge);
    setCapacity(battery->capacity);
    setDesignCapacity(battery->design_capacity);
    setPercentage(battery->percentage);
}

void ROSTopic::setTimeStamp(int timeStamp_)
{
    if(_timeStamp != timeStamp_)
    {
        _timeStamp = timeStamp_;
        emit timeStampChanged();
    }
}

void ROSTopic::setFrequency(double frequency_)
{
    if(_frequency != frequency_)
    {
        _frequency = frequency_;
        emit frequencyChanged();
    }
}

void ROSTopic::setPositionX(double positionX_)
{
    if(_positionX != positionX_)
    {
        _positionX = positionX_;
        emit positionXChanged();
    }
}

void ROSTopic::setPositionY(double positionY_)
{
    if(_positionY != positionY_)
    {
        _positionY = positionY_;
        emit positionYChanged();
    }
}

void ROSTopic::setYaw(double yaw_)
{
    if(_yaw != yaw_)
    {
        _yaw = yaw_;
        emit yawChanged();
    }
}

void ROSTopic::setLinearX(double linearX_)
{
    if(_linearX != linearX_)
    {
        _linearX = linearX_;
        emit linearXChanged();
    }
}

void ROSTopic::setAngularZ(double angularZ_)
{
    if(_angularZ != angularZ_)
    {
        _angularZ = angularZ_;
        emit angularZChanged();
    }
}

void ROSTopic::setAngularX(double angularX_)
{
    if(_angularX != angularX_)
    {
        _angularX = angularX_;
        emit angularXChanged();
    }
}

void ROSTopic::setLeftrmp(int leftrmp_)
{
    if(_leftrmp != leftrmp_)
    {
        _leftrmp = leftrmp_;
        emit leftrmpChanged();
    }
}

void ROSTopic::setRightrmp(int rightrmp_)
{
    if(_rightrmp != rightrmp_)
    {
        _rightrmp = rightrmp_;
        emit rightrmpChanged();
    }
}

void ROSTopic::setVoltage(double voltage_)
{
    if(_voltage!= voltage_)
    {
        _voltage = voltage_;
        emit voltageChanged();
    }
}

void ROSTopic::setCurrent(double current_)
{
    if(_current != current_)
    {
        _current = current_;
        emit currentChanged();
    }
}

void ROSTopic::setCharge(double charge_)
{
    if(_charge != charge_)
    {
        _charge = charge_;
        emit chargeChanged();
    }
}

void ROSTopic::setCapacity(double capacity_)
{
    if(_capacity != capacity_)
    {
        _capacity = capacity_;
        emit capacityChanged();
    }
}

void ROSTopic::setDesignCapacity(double designCapacity_)
{
    if(_designCapacity != designCapacity_)
    {
        _designCapacity = designCapacity_;
        emit designCapacityChanged();
    }
}

void ROSTopic::setPercentage(double percentage_)
{
    if(_percentage != percentage_)
    {
        _percentage = percentage_;
        emit percentageChanged();
    }
}
